Dinámica de un robot manipulador planar de 2 grados de libertad rotacionales (sin control).

In [1]:

    

from sympy import var, sin, cos, Matrix, Integer, eye, Function, Rational, exp, Symbol, I, solve, pi, trigsimp, dsolve, sinh, cosh, simplify
from sympy.physics.mechanics import mechanics_printing
mechanics_printing()


    

In [2]:

    

var("m1 m2 J1 J2 l1 l2 L1 L2 t g")


    

    
Out[2]:





$$\left ( m_{1}, \quad m_{2}, \quad J_{1}, \quad J_{2}, \quad l_{1}, \quad l_{2}, \quad L_{1}, \quad L_{2}, \quad t, \quad g\right )$$

In [3]:

    

q1 = Function("q1")(t)
q2 = Function("q2")(t)


    

In [4]:

    

x1 = l1*cos(q1)
y1 = l1*sin(q1)
v1 = x1.diff("t")**2 + y1.diff("t")**2
v1.trigsimp()


    

    
Out[4]:





$$l_{1}^{2} \dot{q}_{1}^{2}$$

In [5]:

    

x2 = L1*cos(q1) + l2*cos(q1 + q2)
y2 = L1*sin(q1) + l2*sin(q1 + q2)
v2 = x2.diff("t")**2 + y2.diff("t")**2
v2.trigsimp()


    

    
Out[5]:





$$L_{1}^{2} \dot{q}_{1}^{2} + 2 L_{1} l_{2} \operatorname{cos}\left(q_{2}\right) \dot{q}_{1}^{2} + 2 L_{1} l_{2} \operatorname{cos}\left(q_{2}\right) \dot{q}_{1} \dot{q}_{2} + l_{2}^{2} \dot{q}_{1}^{2} + 2 l_{2}^{2} \dot{q}_{1} \dot{q}_{2} + l_{2}^{2} \dot{q}_{2}^{2}$$

In [6]:

    

ω1, ω2 = q1.diff("t"), q1.diff("t") + q2.diff("t")


    

In [7]:

    

K1 = Rational(1, 2)*m1*v1 + Rational(1, 2)*J1*ω1**2
K1


    

    
Out[7]:





$$\frac{J_{1} \dot{q}_{1}^{2}}{2} + \frac{m_{1}}{2} \left(l_{1}^{2} \operatorname{sin}^{2}\left(q_{1}\right) \dot{q}_{1}^{2} + l_{1}^{2} \operatorname{cos}^{2}\left(q_{1}\right) \dot{q}_{1}^{2}\right)$$

In [8]:

    

K2 = Rational(1, 2)*m2*v2 + Rational(1, 2)*J2*ω2**2
K2


    

    
Out[8]:





$$\frac{J_{2}}{2} \dot{q}_{1} + \dot{q}_{2}^{2} + \frac{m_{2}}{2} \left(\left(- L_{1} \operatorname{sin}\left(q_{1}\right) \dot{q}_{1} - l_{2} \left(\dot{q}_{1} + \dot{q}_{2}\right) \operatorname{sin}\left(q_{1} + q_{2}\right)\right)^{2} + \left(L_{1} \operatorname{cos}\left(q_{1}\right) \dot{q}_{1} + l_{2} \left(\dot{q}_{1} + \dot{q}_{2}\right) \operatorname{cos}\left(q_{1} + q_{2}\right)\right)^{2}\right)$$

In [9]:

    

U1 = m1*g*y1
U1


    

    
Out[9]:





$$g l_{1} m_{1} \operatorname{sin}\left(q_{1}\right)$$

In [10]:

    

U2 = m2*g*y2
U2


    

    
Out[10]:





$$g m_{2} \left(L_{1} \operatorname{sin}\left(q_{1}\right) + l_{2} \operatorname{sin}\left(q_{1} + q_{2}\right)\right)$$

In [11]:

    

K = K1 + K2
K


    

    
Out[11]:





$$\frac{J_{1} \dot{q}_{1}^{2}}{2} + \frac{J_{2}}{2} \dot{q}_{1} + \dot{q}_{2}^{2} + \frac{m_{1}}{2} \left(l_{1}^{2} \operatorname{sin}^{2}\left(q_{1}\right) \dot{q}_{1}^{2} + l_{1}^{2} \operatorname{cos}^{2}\left(q_{1}\right) \dot{q}_{1}^{2}\right) + \frac{m_{2}}{2} \left(\left(- L_{1} \operatorname{sin}\left(q_{1}\right) \dot{q}_{1} - l_{2} \left(\dot{q}_{1} + \dot{q}_{2}\right) \operatorname{sin}\left(q_{1} + q_{2}\right)\right)^{2} + \left(L_{1} \operatorname{cos}\left(q_{1}\right) \dot{q}_{1} + l_{2} \left(\dot{q}_{1} + \dot{q}_{2}\right) \operatorname{cos}\left(q_{1} + q_{2}\right)\right)^{2}\right)$$

In [12]:

    

U = U1 + U2
U


    

    
Out[12]:





$$g l_{1} m_{1} \operatorname{sin}\left(q_{1}\right) + g m_{2} \left(L_{1} \operatorname{sin}\left(q_{1}\right) + l_{2} \operatorname{sin}\left(q_{1} + q_{2}\right)\right)$$

In [13]:

    

L = (K - U).expand().simplify()
L


    

    
Out[13]:





$$\frac{J_{1} \dot{q}_{1}^{2}}{2} + \frac{J_{2} \dot{q}_{1}^{2}}{2} + J_{2} \dot{q}_{1} \dot{q}_{2} + \frac{J_{2} \dot{q}_{2}^{2}}{2} + \frac{L_{1}^{2} m_{2}}{2} \dot{q}_{1}^{2} - L_{1} g m_{2} \operatorname{sin}\left(q_{1}\right) + L_{1} l_{2} m_{2} \operatorname{cos}\left(q_{2}\right) \dot{q}_{1}^{2} + L_{1} l_{2} m_{2} \operatorname{cos}\left(q_{2}\right) \dot{q}_{1} \dot{q}_{2} - g l_{1} m_{1} \operatorname{sin}\left(q_{1}\right) - g l_{2} m_{2} \operatorname{sin}\left(q_{1} + q_{2}\right) + \frac{l_{1}^{2} m_{1}}{2} \dot{q}_{1}^{2} + \frac{l_{2}^{2} m_{2}}{2} \dot{q}_{1}^{2} + l_{2}^{2} m_{2} \dot{q}_{1} \dot{q}_{2} + \frac{l_{2}^{2} m_{2}}{2} \dot{q}_{2}^{2}$$

In [14]:

    

τ1 = (L.diff(q1.diff(t)).diff(t) - L.diff(q1)).simplify().expand().collect(q1.diff(t).diff(t)).collect(q2.diff(t).diff(t))


    

In [15]:

    

τ2 = (L.diff(q2.diff(t)).diff(t) - L.diff(q2)).simplify().expand().collect(q1.diff(t).diff(t)).collect(q2.diff(t).diff(t))


    

In [16]:

    

τ1


    

    
Out[16]:





$$L_{1} g m_{2} \operatorname{cos}\left(q_{1}\right) - 2 L_{1} l_{2} m_{2} \operatorname{sin}\left(q_{2}\right) \dot{q}_{1} \dot{q}_{2} - L_{1} l_{2} m_{2} \operatorname{sin}\left(q_{2}\right) \dot{q}_{2}^{2} + g l_{1} m_{1} \operatorname{cos}\left(q_{1}\right) + g l_{2} m_{2} \operatorname{cos}\left(q_{1} + q_{2}\right) + \left(J_{2} + L_{1} l_{2} m_{2} \operatorname{cos}\left(q_{2}\right) + l_{2}^{2} m_{2}\right) \ddot{q}_{2} + \left(J_{1} + J_{2} + L_{1}^{2} m_{2} + 2 L_{1} l_{2} m_{2} \operatorname{cos}\left(q_{2}\right) + l_{1}^{2} m_{1} + l_{2}^{2} m_{2}\right) \ddot{q}_{1}$$

In [17]:

    

τ2


    

    
Out[17]:





$$L_{1} l_{2} m_{2} \operatorname{sin}\left(q_{2}\right) \dot{q}_{1}^{2} + g l_{2} m_{2} \operatorname{cos}\left(q_{1} + q_{2}\right) + \left(J_{2} + l_{2}^{2} m_{2}\right) \ddot{q}_{2} + \left(J_{2} + L_{1} l_{2} m_{2} \operatorname{cos}\left(q_{2}\right) + l_{2}^{2} m_{2}\right) \ddot{q}_{1}$$

In [18]:

    

from scipy.integrate import odeint
from numpy import linspace


    

In [19]:

    

def pendulo_doble(estado, tiempo):
    # Se importan funciones necesarias
    from numpy import sin, cos, matrix
    # Se desenvuelven variables del estado y tiempo
    q1, q2, q̇1, q̇2 = estado
    t = tiempo
    
    # Se declaran constantes del sistema
    m1, m2 = 0.3, 0.5
    l1, l2 = 0.3, 0.2
    L1, L2 = 0.5, 0.4
    J1, J2 = 0.017, 0.005
    g = 9.81
    
    # Señales de control nulas
    tau1, tau2 = 0, 0
    
    # Se calculan algunos terminos comunes
    μ1 = m2*l2**2
    μ2 = m2*L1*l2
    c1, c2, s2 = cos(q1), cos(q2), sin(q2)
    c12 = cos(q1 + q2)
    
    # Se calculan las matrices de masas, Coriolis,
    # y vectores de gravedad, control, posicion y velocidad
    M = matrix([[m1*l1**2 + m2*L1**2 + μ1 + 2*μ2*c2 + J1 + J2, μ1 + μ2*c2 + J2],
                [μ1 + μ2*c2 + J2, μ1 + J2]])
    
    C = -μ2*s2*matrix([[2*q̇2, q̇2],
                       [-q̇1, 0]])
    
    G = matrix([[m1*l1*c1 + m2*L1*c1 + m2*l2*c12],
                [m2*l2*c12]])
    
    Tau = matrix([[tau1],
                  [tau2]])
    
    q = matrix([[q1],
                [q2]])
    
    q̇ = matrix([[q̇1],
                [q̇2]])
    
    # Se calcula la derivada del estado del sistema
    qp1 = q̇1
    qp2 = q̇2
    
    qpp = M.I*(Tau - C*q̇ - G)
    qpp1, qpp2 = qpp.tolist()
    
    return [qp1, qp2, qpp1[0], qpp2[0]]


    

In [20]:

    

tiempos = linspace(0, 10, 1000)
cond_iniciales = [0, 0, 0, 0]
estados_simulados = odeint(func=pendulo_doble, y0=cond_iniciales, t=tiempos)


    

In [21]:

    

q1, q2, q̇1, q̇2 = list(zip(*estados_simulados.tolist()))


    

In [22]:

    

%matplotlib inline
from matplotlib.pyplot import plot, style, figure
from mpl_toolkits.mplot3d import Axes3D
style.use("ggplot")


    

In [23]:

    

fig1 = figure(figsize=(16, 8))

ax1 = fig1.gca()

p1, = ax1.plot(tiempos, q1)
p2, = ax1.plot(tiempos, q2)
ax1.legend([p1, p2],[r"$q_1$", r"$q_2$"])
#ax1.set_ylim(-4.1, 6.1)
ax1.set_xlim(-0.1, 10.1);


    

In [24]:

    

fig1 = figure(figsize=(8, 8))

ax1 = fig1.gca()

p1, = ax1.plot(q1, q2)
ax1.set_ylim(-6.1, 6.1)
ax1.set_xlim(-6.1, 6.1);


    

In [25]:

    

from numpy import sin, cos, arange
from matplotlib import animation, rc
rc('animation', html='html5')


    

In [26]:

    

L1, L2 = 0.5, 0.4
# Se define el tamaño de la figura
fig = figure(figsize=(10, 10))

# Se define una sola grafica en la figura y se dan los limites de los ejes x y y
axi = fig.add_subplot(111, autoscale_on=False, xlim=(-1.1, 1.1), ylim=(-1.1, 1.1))
axi.set_xticklabels([])
axi.set_yticklabels([])
axi.axes.get_xaxis().set_visible(False)
axi.axes.get_yaxis().set_visible(False)

# Se utilizan graficas de linea para el eslabon del pendulo
linea, = axi.plot([], [], "-o", lw=2, color='gray')

def init():
    # Esta funcion se ejecuta una sola vez y sirve para inicializar el sistema
    linea.set_data([], [])
    return linea

def animate(i):
    # Esta funcion se ejecuta para cada cuadro del GIF
    
    # Se obtienen las coordenadas x y y para el eslabon
    xs, ys = [[0, L1*cos(q1[i]), L1*cos(q1[i]) + L2*cos(q1[i]+q2[i])],
              [0, L1*sin(q1[i]), L1*sin(q1[i]) + L2*sin(q1[i]+q2[i])]]
    linea.set_data(xs, ys)
    
    return linea

# Se hace la animacion dandole el nombre de la figura definida al principio, la funcion que
# se debe ejecutar para cada cuadro, el numero de cuadros que se debe de hacer, el periodo 
# de cada cuadro y la funcion inicial
ani = animation.FuncAnimation(fig, animate, arange(1, len(q1)), interval=10, init_func=init);
ani


    

    
Out[26]:











    

In [ ]: